package com.hitqz.robot.biz.service.impl;

import cn.hutool.core.collection.CollectionUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.WorkStatusEnum;
import com.hitqz.robot.api.common.dto.warning.AddWarningDto;
import com.hitqz.robot.api.common.entity.MapArea;
import com.hitqz.robot.api.common.entity.MapAreaPos;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.business.util.CalculateUtil;
import com.hitqz.robot.biz.service.*;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.Point;
import org.locationtech.jts.geom.Polygon;
import org.springframework.stereotype.Service;

import java.util.List;
import java.util.Optional;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicReference;
import java.util.concurrent.locks.ReentrantLock;

/**
 * @author xupkun
 * @date 2024/7/31
 */
@Service
@RequiredArgsConstructor
@Slf4j
public class BusinessServiceImpl implements BusinessService {

    private final RobotMapPosService robotMapPosService;
    private final RobotDriverService robotDriverService;
    private final RobotService robotService;
    private final MapAreaService mapAreaService;
    private final MapAreaActionService mapAreaActionService;
    private final AreaActionService areaActionService;
    private final MapAreaPosService mapAreaPosService;
    private final DispatchService dispatchService;
    private GeometryFactory geometryFactory = new GeometryFactory();

    private ReentrantLock cancelLock = new ReentrantLock();

    @Override
    public boolean goCharge(boolean webCtrl, boolean forceCharge) {
        synchronized (this) {
            Robot robot = robotService.getDefaultRobot();
            if (robot == null) {
                log.error("Robot is null, cannot proceed with charging.");
                return false;
            }

            IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
            int chargeThreshold = Optional.ofNullable(robot.getChargeThreshold()).orElse(20);

            if (iRobotPosDto == null || chargeThreshold < 0) {
                return false;
            }

            // 简化条件表达式
            boolean shouldCharge = (iRobotPosDto.getBatteryLevel() > 0 &&iRobotPosDto.getBatteryLevel() <= chargeThreshold) || webCtrl;

            if (shouldCharge) {
                if (!forceCharge && !robot.getWorkStatus().equals(WorkStatusEnum.FREE.getCode())) {
                    log.info("Robot is not free, cannot charge: {}, {}", robot.getWorkStatus(), WorkStatusEnum.CMD_ROBOT_STATUS.get(robot.getWorkStatus()));
                    return false;
                }

                log.info("Battery level low: {} (threshold: {}) - Web control: {}", iRobotPosDto.getBatteryLevel(), chargeThreshold, webCtrl);

                if (!iRobotPosDto.isCharging()) {
                    RobotMapPos chargingPos = findChargePos();
                    if (chargingPos!= null) {
                        //
                        if (dispatchService.go2TargetPosition(chargingPos , true)) {
                            robot.setWorkStatus(WorkStatusEnum.WAY_2_CHARGE.getCode());
                            robotService.updateById(robot);
                        }
                    } else {
                        AddWarningDto addWarningDto = new AddWarningDto();
                        addWarningDto.setCode("0");
                        addWarningDto.setTitle("Vehicle Alarm");
                        addWarningDto.setContent("No charging area configured.");
                        addWarningDto.setSuggestion("请检查充电区域是否正确");
                        addWarningDto.setLevel(3);
                        SpringUtil.getApplicationContext().publishEvent(addWarningDto);
                    }
                } else {
                    log.info("Robot is already charging.");
                }
            }
        }


        return true;
    }

    @Override
    public boolean cancelCharge() {
        try {
            if (cancelLock.tryLock(2, TimeUnit.SECONDS)) {
                try {
                    IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
                    if (iRobotPosDto == null) {
                        log.warn("取消充电失败：无法获取机器人位置信息");
                        return false;
                    }
                    //if (!iRobotPosDto.isCharging()) {
                    //    log.info("机器人当前不在充电状态");
                    //    return false;
                    //}
                    Robot robot = robotService.getDefaultRobot();
                    if (robot == null) {
                        log.warn("取消充电失败：无法获取默认机器人信息");
                        return false;
                    }
                    RobotMapPos robotMapPos = findChargePos();
                    if (robotMapPos == null ) {
                        log.info("取消充电失败：无法找到充电点");
                        return false;
                    }
                    //String lastStation = iRobotPosDto.getLastStation();
                    //RobotMapPos robotMapPos = StrUtil.isNotBlank(lastStation)
                    //        ? robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery().eq(RobotMapPos::getCode, lastStation))
                    //        : null;
                    //if (robotMapPos == null) {
                    //    log.warn("取消充电失败：无法找到充电点 {}", lastStation);
                    //    return false;
                    //}
                    RobotMapPos prePos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                            .like(RobotMapPos::getPreCode, robotMapPos.getCode())
                            .eq(RobotMapPos::getMapCode, robot.getMapCode()).last(" limit 1"));
                    if (prePos != null) {
                        dispatchService.go2TargetPosition(prePos, false);
                        log.info("取消充电前往前置点 {}", prePos.getCode());
                        Thread.sleep(5000);
                    } else {
                        log.warn("取消充电前往起始点，没有找到前置点");
                        return false;
                    }
                } catch (Exception e) {

                }finally {
                    cancelLock.unlock();
                }
                return true;
            }
        } catch (InterruptedException e) {
            log.error("取消充电过程中发生异常", e);
            return false;
        }
        return false;
    }

    @Override
    public RobotMapPos findChargePos() {
        Robot robot = robotService.getDefaultRobot();
        List<MapArea> mapAreas = mapAreaService.findMapAreaActions("charging")
                .stream()
                .filter(t -> t.getStatus() == 1 && t.getMapCode().equals(robot.getMapCode()))
                .toList();

        if (mapAreas.isEmpty()) {
            log.info("No charging area configured for the current map.");
            return null;
        }

        // Find the closest charging position
        List<RobotMapPos> mapPos = robotMapPosService.list(Wrappers.<RobotMapPos>lambdaQuery().eq(RobotMapPos::getMapCode, robot.getMapCode()));
        AtomicReference<RobotMapPos> chargingPos = new AtomicReference<>();

        for (MapArea mapArea : mapAreas) {
            List<MapAreaPos> mapAreaPosList = mapAreaPosService.list(Wrappers.<MapAreaPos>lambdaQuery()
                    .eq(MapAreaPos::getAreaId, mapArea.getId()));

            if (CollectionUtil.isEmpty(mapAreaPosList) || mapAreaPosList.size() < 4) {
                log.warn("Map area not properly configured: {}", mapArea.getId());
                continue;
            }
            Polygon polygon =  CalculateUtil.createAreaPolygon(mapAreaPosList);
            for (RobotMapPos pos : mapPos) {
                Point point = geometryFactory.createPoint(new Coordinate(pos.getPosX(), pos.getPosY()));
                boolean inPolygon = polygon.intersects(point);
                log.info("Polygon intersects: {}-- {}-- {} ", pos.getName(), mapArea.getName(), inPolygon);
                if (inPolygon) {
                    chargingPos.set(pos);
                    break;
                }
            }
        }

        log.info("Selected charging position: {}", chargingPos);
        return chargingPos.get();
    }


}
